import time
import math
from ch347T.ch347 import CH347
import threading

# ============================================================================
# Raspi PCA9685 16-Channel PWM Servo Driver
# ============================================================================


class PCA9685Thread(threading.Thread):
    # Registers/etc.
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09
    __ALLLED_ON_L = 0xFA
    __ALLLED_ON_H = 0xFB
    __ALLLED_OFF_L = 0xFC
    __ALLLED_OFF_H = 0xFD

    def __init__(self, work_queue, address=0x40, frequency=50, debug=False, dev_index=0):
        """
        初始化对象
        :param work_queue:信息队列，用于将主线程中的控制信息发送至子线程
        :param address: i2c地址
        :param frequency: PWM频率，默认50用于驱动舵机，如果驱动电机最好改为500HZ以上。
        :param debug: 是否显示调试信息。
        """
        super().__init__()
        # self.bus = smbus.SMBus(1)
        # self.usb_iic = CH341AIIC()
        self.work_queue = work_queue
        self.usb_iic = CH347(device_index=dev_index)
        self.usb_iic.open_device()
        self.usb_iic.i2c_set(3)
        self.address = address
        self.debug = debug
        self.frequency = frequency
        self.write_cache = {}  # 初始化字典用于缓存写入内容
        if self.debug:
            print("Resetting PCA9685")
        self.write(self.__MODE1, 0x00)
        self.setPWMFreq(self.frequency)

    def run(self):
        while True:
            item = self.work_queue.get()
            if item is None:  # 约定一个特殊的值表示线程应该退出
                break
            # print(f"Processing {item}")
            self.setPWM(item[0], item[1], item[2])
            # self.work_queue.task_done()

    def write(self, reg, value):
        """
        Writes an 8-bit value to the specified register/address
        :param reg:
        :param value:
        :return:
        """
        # self.bus.write_byte_data(self.address, reg, value)

        # result = self.usb_iic.write_stream(self.address, reg, value)
        write_data = bytes([(self.address << 1) | 0, reg, value])
        result = self.usb_iic.stream_i2c(write_data, 0)
        if self.debug:
            print("I2C: Device write 0x%02X to reg 0x%02X return 0x%02X" % (value, reg, result))
            pass
        time.sleep(0.0005)

    def read(self, reg):
        """
        Read an unsigned byte from the I2C device
        :param reg:
        :return:
        """
        # result = self.bus.read_byte_data(self.address, reg)
        # result = self.usb_iic.read_stream(self.address, reg)
        write_data = bytes([(self.address << 1) | 0, reg])
        result = self.usb_iic.stream_i2c(write_data, 1)
        if self.debug:
            print("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % (self.address, result[0], reg))
        return result[0]

    def setPWMFreq(self, freq):
        """
        Sets the PWM frequency
        :param freq: 频率
        :return:
        """
        prescaleval = 25000000.0  # 25MHz
        prescaleval /= 4096.0  # 12-bit
        prescaleval /= float(freq)
        prescaleval -= 1.0
        if self.debug:
            print("Setting PWM frequency to %d Hz" % freq)
            print("Estimated pre-scale: %d" % prescaleval)
        prescale = math.floor(prescaleval + 0.5)
        if self.debug:
            print("Final pre-scale: %d" % prescale)

        oldmode = self.read(self.__MODE1)
        newmode = (oldmode & 0x7F) | 0x10  # sleep
        self.write(self.__MODE1, newmode)  # go to sleep
        self.write(self.__PRESCALE, int(math.floor(prescale)))
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x80)

    def setPWM(self, channel, on, off):
        """
        Sets a single PWM channel
        :param channel:
        :param on:
        :param off:
        :return:
        """
        # 计算需要写入的地址
        on_low_addr = self.__LED0_ON_L + 4 * channel
        on_high_addr = self.__LED0_ON_H + 4 * channel
        off_low_addr = self.__LED0_OFF_L + 4 * channel
        off_high_addr = self.__LED0_OFF_H + 4 * channel

        # 准备要写入的值（确保是16位）
        on_low = on & 0xFF
        on_high = (on >> 8) & 0xFF
        off_low = off & 0xFF
        off_high = (off >> 8) & 0xFF

        # 构造缓存键
        cache_keys = [
            (on_low_addr, on_low),
            (on_high_addr, on_high),
            (off_low_addr, off_low),
            (off_high_addr, off_high),
        ]

        # 检查是否需要写入
        for addr, value in cache_keys:
            if addr not in self.write_cache or self.write_cache[addr] != value:
                # 如果地址和值不在缓存中，或者值已改变，则写入
                self.write(addr, value)
                # 更新缓存
                self.write_cache[addr] = value
                if self.debug:
                    print(f"Writing to {addr}: {value}")
                    # if self.debug:
        #     print("channel: %d  LED_ON: %d LED_OFF: %d" % (channel, on, off))
        #
        # self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF)
        # self.write(self.__LED0_ON_H + 4 * channel, on >> 8)
        # self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF)
        # self.write(self.__LED0_OFF_H + 4 * channel, off >> 8)

    def close_device(self):
        self.usb_iic.close_device()